#include "ConstrainedObject.h"

ConstrainedObject::ConstrainedObject(TextureManager * textureManager,
					Vector2D position,
					Vector2D * vertices,
					int numVertices,
					float mass,
					bool isGrabbable,
					bool freeRotate,
					Vector2D movementAxis,
					Vector2D movementEndpoints, 
					char * textureName, Vector2D * textureVertices)
:Object(textureManager, position, vertices, numVertices, mass, isGrabbable)
{
	cantShift = true;
	this->freeRotate = freeRotate;
	this->movementAxis  = !movementAxis;
	this->textureVertices = textureVertices;
	rightMovementEndpoint = position + ((!movementAxis) * movementEndpoints.x);
	leftMovementEndpoint = position + ((!movementAxis) * -movementEndpoints.y);
	if(textureManager)
	{
		if(!textureManager->LoadTexture(textureName))
		{
			printf("Failed to load %s\n", textureName);
		}
		else
		{
			material->SetTextureIndex(textureManager->TextureIndex(textureName));
		}
	}
}
ConstrainedObject::~ConstrainedObject()
{
	
}
void ConstrainedObject::AddForceObjectSpace(Vector2D ray, Vector2D origin)
{
	if(isPinned)
		return;
	if(ray.Magnitude() == 0.0)
		return;
	////printf("ray = (%f, %f)\n",ray.x, ray.y);
	////printf("origin = (%f, %f)\n",origin.x, origin.y);
	
	if(freeRotate)
	{
		Vector2D towardCenter = -1.0 * origin;
		Vector2D unitTowardCenter = !towardCenter; 
		Vector2D unitRay = !ray;
		
		
		float cosTheta = unitTowardCenter.dot(ray);//unitTowardCenter.dot(ray);
		
		Vector2D rayTowardCenter = (cosTheta) * unitTowardCenter;
		AddForce(rayTowardCenter);
	
		Vector2D rayRotational = (ray - rayTowardCenter);
		
		
		torque += rayRotational.x * towardCenter.y - rayRotational.y * towardCenter.x;
	}
	else
	{
		AddForce(ray);	
	}
}
void ConstrainedObject::AddForceObjectSpace(Vector2D ray, Vector2D origin, bool groundForce)
{
	AddForceObjectSpace(ray, origin);	
}
void ConstrainedObject::Think(double elapsedTime)
{
	
		
	if(PreThinkEntryPoint(elapsedTime))
	{
		printf("Escaping think\n");
		return;
	}
		
	if(isPinned)
	{
		velocity = Vector2D(0,0);
		return;
	}
	
	if(mass != 0.0)
	{
		acceleration = translationalForce / mass;
	}
	else
	{
		acceleration = Vector2D(0,0);
	}
	
	acceleration = acceleration.dot(movementAxis) * movementAxis;
	
	Vector2D fromRight = position-rightMovementEndpoint;
	Vector2D fromLeft = leftMovementEndpoint - rightMovementEndpoint;
	float pos = (fromRight).dot(movementAxis)/ (fromLeft).dot(movementAxis);
	
	float axisVel = velocity.dot(movementAxis);
	//if((velocity - (movementAxis * axisVel)).Magnitude() > 0)
	//	printf("HOW AM I MOVING SIDEWAYS??\n");
	/*if(pos < 0.1)
	{
		//accelerate in the direction of the axis
		
		if(axisVel > 0)
			acceleration = movementAxis * -axisVel/elapsedTime;
		
		printf("Here1: acceleration.Mag = %f\n", acceleration.Magnitude());
		
		if(pos < 0)
			position= rightMovementEndpoint;
		
	}
	else if (pos > 0.9)
	{
		//accelerate anti axis
		if(axisVel < 0)
			acceleration = movementAxis * -axisVel/elapsedTime;
		printf("Here2: acceleration.Mag = %f\n", acceleration.Magnitude());
		if(pos > 1.0)
			position = leftMovementEndpoint;
	}*/
	
	
	
	if(angularMass != 0.0)
	{
		angularAcceleration = torque / angularMass;
	}
	else
	{
		angularAcceleration = 0.0;
	}
		
		velocity = velocity + (acceleration * elapsedTime);
		//if(velocity.Magnitude() > 1000)
		//	velocity = !velocity * 1000.0;

		//if((velocity.Magnitude() * elapsedTime) > 0.01)
		position = position + (velocity * elapsedTime);
			
		fromRight = position-rightMovementEndpoint;
		fromLeft = leftMovementEndpoint - rightMovementEndpoint;	
		pos = (fromRight).dot(movementAxis)/(fromLeft).dot(movementAxis);
		if(pos < 0)
		{
			position = rightMovementEndpoint;
			velocity = Vector2D(0,0);
		}
		if(pos > 1)
		{
			position = leftMovementEndpoint;
			velocity = Vector2D(0,0);
		}
			
		if(freeRotate)
			angularVelocity = angularVelocity + (angularAcceleration * elapsedTime);
		//if(angularVelocity > 10)
		//	angularVelocity = 10;

		//make sure our position is on the axis
		//Vector2D axis = !(leftMovementEndpoint - rightMovementEndpoint)
		
		ThinkEntryPoint(elapsedTime);
		
		Vector2D rightToPos = position - rightMovementEndpoint;
		float distance = (movementAxis).dot(rightToPos);
		position = movementAxis * distance + rightMovementEndpoint;
	
		translationalForce = Vector2D(0,0);
		torque = 0.0;
		if(hitGround)
		{
			onGroundFilter = 0;
			isOnGround = true;
		}
		else if(isOnGround)
			onGroundFilter++;
		
		if(onGroundFilter > 5)
			isOnGround = false;
			
		hitGround = false;
		isColliding = false;
		isOnRightWall = false;
		isOnLeftWall = false;
		lastElapsedTime = elapsedTime;
}
bool ConstrainedObject::PreRotate(float theta)
{
	return !freeRotate;
}
